#include <rocr6_gazebo/rocr6_gazebo_node.h>
#include <sensor_msgs/JointState.h>
#include <rocr6_msgs/Goal.h>
#include <rocr6_msgs/Feedback.h>
#include <std_msgs/Float64.h>
#include <iostream>
#include <unistd.h>

#define _IS_ACTION_ 0
#if _IS_ACTION_
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <actionlib/client/simple_action_client.h>
#include <trajectory_msgs/JointTrajectory.h>
#endif

#define JOINT_MAX   6
#define JOINT_GAIN  1

static double r_data[JOINT_MAX] = {0, 0, 0, 0, 0, 0};
static double t_data[JOINT_MAX] = {0, 0, 0, 0, 0, 0};

static double last[JOINT_MAX] = {0, 0, 0, 0, 0, 0};

#if _IS_ACTION_
typedef actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> Client;

void doneCb(const actionlib::SimpleClientGoalState& state, const control_msgs::FollowJointTrajectoryResultConstPtr & result)
{
}

void activeCb(void)
{
}

void feedbackCb(control_msgs::FollowJointTrajectoryFeedbackConstPtr & result)
{
}
#endif

//接收用户话题数据
void usrGoal_CB(const rocr6_msgs::Goal::ConstPtr & msg)
{
    //弧度变角度并且数据放大10倍
    last[0] = msg->a1*JOINT_GAIN;
    last[1] = msg->a2*JOINT_GAIN;
    last[2] = msg->a3*JOINT_GAIN;
    last[3] = msg->a4*JOINT_GAIN;
    last[4] = msg->a5*JOINT_GAIN;
    last[5] = msg->a6*JOINT_GAIN;
    if(
        (t_data[0]==last[0])&&
        (t_data[1]==last[1])&&
        (t_data[2]==last[2])&&
        (t_data[3]==last[3])&&
        (t_data[4]==last[4])&&
        (t_data[5]==last[5])
    )
    {
        //发送的角度相同，不用处理
    }
    else
    {
        t_data[0] = last[0];
        t_data[1] = last[1];
        t_data[2] = last[2];
        t_data[3] = last[3];
        t_data[4] = last[4];
        t_data[5] = last[5];
        //控制器插补模式运行
        //net_txSig(1,0,t_data);
    }
}

//接收关节反馈话题数据
void JointState_CB(const sensor_msgs::JointState::ConstPtr & msg)
{
    r_data[0] = msg->position[0]*57.3*JOINT_GAIN;
    r_data[1] = msg->position[1]*57.3*JOINT_GAIN;
    r_data[2] = msg->position[2]*57.3*JOINT_GAIN;
    r_data[3] = msg->position[3]*57.3*JOINT_GAIN;
    r_data[4] = msg->position[4]*57.3*JOINT_GAIN;
    r_data[5] = msg->position[5]*57.3*JOINT_GAIN;
}

int main(int argc, char**argv)
{
    ros::init(argc, argv, "rocr6_gazebo");
    ros::NodeHandle nh;
    ros::Rate loop_rate(100);
    rocr6_msgs::Feedback joint_fb;
    std_msgs::Float64 xjoint;
    int count = 0;

#if _IS_ACTION_
    Client client("/rocr6/arm_joint_controller/follow_joint_trajectory",true);
    ROS_INFO("Waiting for action server to start.");
    client.waitForServer();
    ROS_INFO("Action server started, sending goal.");
    control_msgs::FollowJointTrajectoryGoal Goal;
#endif

    //订阅用户话题
    ros::Subscriber sub_user = nh.subscribe("rocr6_msgs/goal", 1000, usrGoal_CB);
    //订阅关节状态话题
    ros::Subscriber sub_joint = nh.subscribe("rocr6/joint_states", 1000, JointState_CB);
                                                        
    //返回关节实际角度
    ros::Publisher pub_user = nh.advertise<rocr6_msgs::Feedback>("rocr6_msgs/feedback", 1000);
    //发送关节角度
    ros::Publisher pub_j1 = nh.advertise<std_msgs::Float64>("/rocr6/joint1_position_controller/command", 1000);
    ros::Publisher pub_j2 = nh.advertise<std_msgs::Float64>("/rocr6/joint2_position_controller/command", 1000);
    ros::Publisher pub_j3 = nh.advertise<std_msgs::Float64>("/rocr6/joint3_position_controller/command", 1000);
    ros::Publisher pub_j4 = nh.advertise<std_msgs::Float64>("/rocr6/joint4_position_controller/command", 1000);
    ros::Publisher pub_j5 = nh.advertise<std_msgs::Float64>("/rocr6/joint5_position_controller/command", 1000);
    ros::Publisher pub_j6 = nh.advertise<std_msgs::Float64>("/rocr6/joint6_position_controller/command", 1000);
    
    while (ros::ok())
    {
        joint_fb.header.seq = count++;
        if(count>1000)
        {
            count = 0;
        }
        joint_fb.header.stamp = ros::Time::now();
		joint_fb.header.frame_id = "rocr6";

        joint_fb.shoulder_pan_joint.position = r_data[0];  
        joint_fb.shoulder_lift_joint.position = r_data[1]; 
        joint_fb.elbow_joint.position = r_data[2]; 
        joint_fb.wrist_1_joint.position = r_data[3]; 
        joint_fb.wrist_2_joint.position = r_data[4]; 
        joint_fb.wrist_3_joint.position = r_data[5]; 
        pub_user.publish(joint_fb); 

        xjoint.data = t_data[0]*0.01745;
        pub_j1.publish(xjoint);  
        
        xjoint.data = t_data[1]*0.01745;
        pub_j2.publish(xjoint); 

        xjoint.data = t_data[2]*0.01745;
        pub_j3.publish(xjoint); 

        xjoint.data = t_data[3]*0.01745;
        pub_j4.publish(xjoint);

        xjoint.data = t_data[4]*0.01745;
        pub_j5.publish(xjoint);

        xjoint.data = t_data[5]*0.01745;
        pub_j6.publish(xjoint);

        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}
